#include <cmath>
#include <vector>

#include <opencv/cv.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

typedef pcl::PointXYZI PointType;
using std::sin;
using std::cos;
using std::atan2;

const int N_SCANS = 16;
ros::Publisher pubLaserCloud;
int n = 0;
pcl::PointCloud<pcl::PointXYZ> laserCloudSingle;
pcl::PointCloud<pcl::PointXYZ> laserCloudDouble;
pcl::PointCloud<pcl::PointXYZ> laserCloudLine;

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
  sensor_msgs::PointCloud2 laserCloudOutMsg;
  pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
  
  /*
  n++;
  if(n%2)
  {
    laserCloudSingle = laserCloudIn;
  }
  else
  {
    laserCloudDouble = laserCloudSingle;
    laserCloudDouble.width =
  laserCloudSingle.points.size()+laserCloudIn.points.size();
    laserCloudDouble.points.insert(laserCloudDouble.points.end(),laserCloudIn.begin(),laserCloudIn.end());
    pcl::toROSMsg(laserCloudDouble, laserCloudOutMsg);
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
    laserCloudOutMsg.header.frame_id = "/world";
    pubLaserCloud.publish(laserCloudOutMsg);
  }
  */
  laserCloudDouble.width = laserCloudIn.points.size();
  laserCloudDouble.height = 1;
  //ROS_INFO("width is %d", (int)laserCloudDouble.width);
  // laserCloudDouble.points.insert(laserCloudDouble.points.end(),laserCloudIn.begin(),laserCloudIn.end());
  laserCloudDouble.points.assign(laserCloudIn.begin(), laserCloudIn.end());
  //ROS_INFO("size is %d", (int)laserCloudDouble.points.size());
  //pcl::toROSMsg(laserCloudDouble, laserCloudOutMsg);
  //laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
  //laserCloudOutMsg.header.frame_id = "/world";
  //pubLaserCloud.publish(laserCloudOutMsg);

  pcl::PointXYZ point;
  std::vector<pcl::PointCloud<pcl::PointXYZ> > laserCloudScans(N_SCANS);
  float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
  ROS_INFO("start_x=%f,start_y=%f,start_z=%f",laserCloudIn.points[0].x,laserCloudIn.points[0].y,laserCloudIn.points[0].z);
  ROS_INFO("startOri=%f",startOri);
  int count =0;
  for (int i = 0; i < laserCloudDouble.points.size(); i++) {
    point.x = laserCloudIn.points[i].x;
    point.y = laserCloudIn.points[i].y;
    point.z = laserCloudIn.points[i].z;
    float temp = sqrt(point.x * point.x + point.y * point.y  + point.z * point.z);
    float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) *180 / 3.1415926;
    float Ori = -atan2(point.y, point.x);
    if(temp == 0)
     count++;
      //ROS_INFO("x=%f,y=%f,z=%f",point.x,point.y,point.z);
    int scanID;
    int roundedAngle = int(angle + (angle < 0.0 ? -0.5 : +0.5)) + 4;
    
    //int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5));
    if(i==0)
      ROS_INFO("roundangle1=%d",roundedAngle);
        if(i==1)
      ROS_INFO("roundangle2=%d",roundedAngle);
          if(i==2)
      ROS_INFO("roundangle3=%d",roundedAngle);
          if(i==3)
      ROS_INFO("roundangle4=%d",roundedAngle);
          if(i==4)
      ROS_INFO("roundangle5=%d",roundedAngle);
    scanID = (roundedAngle+15)/2;
    if (scanID > (N_SCANS - 1) || scanID < 0 ){
      continue;
    }
    ROS_INFO("scanID:%d",scanID);

    laserCloudScans[scanID].push_back(point);
  }
  int count_lines=0;
  for(int i=0;i<16;i++)
  {
    count_lines+=laserCloudScans[i].size();
  }
  ROS_INFO("count of lines=%d",count_lines);

  laserCloudLine.width = laserCloudScans[0].size()+laserCloudScans[1].size();
  laserCloudLine.height =1;
  laserCloudLine.points.assign(laserCloudScans[0].begin(), laserCloudScans[0].end());
  laserCloudLine.points.insert(laserCloudLine.points.end(),laserCloudScans[1].begin(),laserCloudScans[1].end());
  pcl::toROSMsg(laserCloudLine, laserCloudOutMsg);
  laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
  laserCloudOutMsg.header.frame_id = "/world";
  pubLaserCloud.publish(laserCloudOutMsg);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "scan_merge");
  ros::NodeHandle nh;
  
  ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>
                               ("/rfans_driver/rfans_points", 2,
  laserCloudHandler);

  /*
  ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(
      "/velodyne_points", 2, laserCloudHandler);
*/
  pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/point_cloud_out", 2);
  ros::spin();
  return 0;
}
